#!/usr/bin/env python
# -*- encoding: utf-8 -*-
"""
@Description:       :
@Date     :2021/03/25 21:21:03
@Author      :chenqi
@version      :1.0
"""
from shapely.geometry import Polygon as Rectangle
from typing import Dict, List
import numpy as np
import sys

sys.path.append(sys.path[0] + "/../")


class CarInfo:
    # def __init__(self, heading: float, lon: float,
    #              lat: float, vin: str) -> None:
    #     self.heading = heading
    #     self.lon = lon
    #     self.lat = lat
    #     self.vin = vin
    #     self.x = 0
    #     self.y = 0
    def __init__(self, root) -> None:
        """
        heading: 车头朝向
        rear_wheel_x,y: 车辆后轴中心点坐标
        angle: ？
        speed: 车辆速度
        accelerate: 车辆加速度，经纬度加速度==勾股定理=>加速度


        Args:
            root ([type]): [description]
        """
        from utils.latlontomap import transform, calc_accelerate
        self.heading = root["heading"]
        self.rear_wheel_x, self.rear_wheel_y, self.heading = transform(
            # self.heading
            lon=root["longitude"], lat=root["latitude"], angle=0
        )
        self.speed = root["car_speed"]
        self.accelerate = calc_accelerate(
            root["longtitudinalaccl"], root["lateraaccel"]
        )
        self.angle = root["wheel_angle"]
        self.gear = root["gear"]
        self.brakehold = root["brake"]

    def get_gear(self):
        if self.gear == 0:
            return "No Request"
        elif self.gear == 1:
            return "P"
        elif self.gear == 2:
            return "R"
        elif self.gear == 3:
            return "N"
        elif self.gear == 4:
            return "D"
        return "Reserved"

    def get_brakehold(self):
        return "lock" if self.brakehold == 1 else "release"

    def get_car(self):
        res = self.__dict__
        res.pop("gear")
        res.pop("brakehold")
        return res

    def get_dict(self) -> Dict:
        # print('car info dict is ', self.__dict__)
        return self.__dict__

    def __str__(self) -> str:
        return str.format(
            "car info:heading=%f, longitude=%f, latitude=%f, vin=%s",
            self.heading,
            self.lon,
            self.lat,
            self.vin,
        )


class Obstacle:
    def __init__(
        self,
        length: float,
        width: float,
        # angle: float,
        heading: float,
        longitude: float,
        latitude: float,
        classification: float,
        score: float,
        # speed: float,
        speed_x_object: float,
        speed_y_object: float,
        height: float = 0.0,
    ) -> None:
        """[summary]

        Args:
            heading (float): 障碍与 # TODO 正北方向逆时针旋转的角度
            longitude (float): #经纬度
            latitude (float): 纬度
            distance (float): 障碍物与自车距离
            score (float): 障碍物类别置信度
            height (float, optional): Defaults to 0..
            rectangle (shapely.geometry.Polygon): 障碍物在世界中由四个顶点组成矩形
        """
        self.length = length
        self.width = width
        # self.angle = angle
        self.heading = heading
        self.longitude = longitude
        self.latitude = latitude
        self.classification = classification
        self.score = score
        # self.speed = speed
        self.speed_x_object = speed_x_object
        self.speed_y_object = speed_y_object
        self.height = height
        self.center_x = 0.0
        self.center_y = 0.0
        self.generate_rect = False
        self.rectangle = Rectangle()

    def set_position(self, center):
        self.center_x = center[0]
        self.center_y = center[1]

    def get_dict(self):
        res = self.__dict__.copy()
        res.pop("rectangle")
        res.pop("generate_rect")
        return res

    def calculate_rectangle(self):
        """根据矩形长宽生成四个顶点
        默认矩形长平行与正北方向
        TODO Python 会修改原list中的obs，不用返回值

        Args:
            obs (Obstacle): 障碍物信息
            Rectangle: 四个顶点组成的矩形，从左上角按顺时针方向生成
        """
        from utils.tools import rotatation_matrix, transform_martix
        from utils.latlontomap import do_transform
        self.generate_rect = True
        rect = Rectangle(
            [
                [-self.width / 2, self.length / 2],
                [self.width / 2, self.length / 2],
                [self.width / 2, -self.length / 2],
                [-self.width / 2, -self.length / 2],
            ]
        )
        new_rect = []
        rect_rotat = rotatation_matrix(self.heading)
        rect_trans = transform_martix(self.center_x, self.center_y)
        for i in range(4):
            rect_point = np.array(rect.exterior.coords[i])
            rect_point = do_transform(p=rect_point, r=rect_rotat, t=rect_trans)
            new_rect.append(rect_point)
        self.rectangle = Rectangle(new_rect)

    def __gt__(self, other):
        """self.score > other.score

        Args:
            other (Obstacle): 其他障碍物

        Returns:
            bool: 当前障碍物self的置信度是否大于被比较障碍物的置信度
        """
        return self.score > other.score

    def __lt__(self, other):
        return self.score < other.score


class VehicleMessage:
    def __init__(self, obstacles: List[Obstacle], car_info: CarInfo = None) -> None:
        """每辆车的自车状态和障碍物列表

        Args:
            car_info (CarInfo): 自车状态.
            obstacles (list): 障碍物列表. 去掉空列表
        """
        # self.car_info = car_info
        self.obstacles = obstacles


class MergeObstacles:
    def __init__(self, vehicle_messages: List[VehicleMessage]) -> None:
        """Initilaizer

        Args:
            vehicle_messages (list): 多个障碍物列表：
            eg.
                [
                    vehicle_message_0,
                    vehicle_message_1,
                    ...,
                    vehicle_message_n
                ]
        """
        self.vehicle_messages = vehicle_messages
        self.obstacle_list = []
        self.obstacle_dict = []
        self.min_x = float("inf")
        self.max_x = float("-inf")
        self.min_y = float("inf")
        self.max_y = float("-inf")

    def obstacles_merge(self):
        """感知融合，将多个障碍物列表融合为一个障碍物列表。
        生成一个融合后的障碍物里列表，其中每个障碍物的坐标都是真实世界的坐标。

        Returns:
            list: 融合后的列表，可以使用self.obstacle_list获取
            dict: 融合后的字典，可以使用self.obstacle_dict获取
        """
        self.__convert_coordnate()
        row = self.__split_area()
        self.__bigger_merge(row)
        # self.__no_split_area()
        # self.__merge()
        self.__to_dict()

    def __convert_coordnate(self):
        """将所有障碍物的中心坐标相对坐标转换为真实世界坐标
        完善障碍物列表中每个障碍物中心的x,y
        """
        from utils.latlontomap import transform

        # 将车的经纬度转换为世界坐标
        for vm in self.vehicle_messages:
            # (vm.car_info.x, vm.car_info.y) = transform(
            #     vm.car_info.lon, vm.car_info.lat)

            for obs in vm.obstacles:
                obs.center_x, obs.center_y, obs.heading = transform(
                    lon=obs.longitude, lat=obs.latitude, angle=obs.heading
                )
                self.min_x = min(self.min_x, obs.center_x)
                self.max_x = max(self.max_x, obs.center_x)
                self.min_y = min(self.min_y, obs.center_y)
                self.max_y = min(self.max_y, obs.center_y)

        """以下部分作废，需要两次转换才能得到真实世界坐标
        现在只需要一次就行
        """
        # # 根据自车的世界坐标将障碍物中心点转换为世界坐标
        # # 根据自车求出平移旋转矩阵
        # rect_rotat = rotatation_matrix(vm.car_info.heading)
        # rect_trans = transform_martix(vm.car_info.x, vm.car_info.y)
        # # 对每个障碍物求其中心点的世界坐标
        # for obs in vm.obstacles:
        #     # TODO
        #     # 中心点原始坐标（自车坐标系下）(0, 0)
        #     obs_rotat = rotatation_matrix(obs.azimuth)
        #     obs_trans = transform_martix(obs.center_x, obs.center_y)
        #     (obs.center_x, obs.center_y) =
        #         do_transform(np.array([
        #             obs.center_x, obs.center_y
        #         ]), r=obs_rotat, t=obs_trans)

        #     # 中心点转换后坐标（世界坐标系下）
        #     (obs.center_x, obs.center_y) = do_transform(np.array([obs.center_x, obs.center_y]),
        #                                                 r=rect_rotat, t=rect_trans)

        # return obs.rectangle

    def __split_area(self):
        """分割由所有障碍物组成的最大区域，按一般车身长款分割成小区域
        生成一个N×M的矩阵，每个元素都有一个障碍物列表
        根据矩阵的index建立map，使用index去访问提高效率
        {
            "index_1":[obs_11, obs_12],
            ...
            "index_n":[obs_n1, obs_n2]
        }
        """
        from math import ceil
        from utils.latlontomap import calc_index, transform

        row = ceil((self.max_x - self.min_x) / 1.59)
        # lin = math.ceil((self.max_y - self.max_x)/4.43)
        self.obstacle_map = {}
        for vm in self.vehicle_messages:
            for obs in vm.obstacles:
                # TODO 计算rect前进行坐标转换
                obs.center_x, obs.center_y, obs.heading = transform(
                    obs.longitude, obs.latitude, obs.heading
                )
                obs_id = calc_index(
                    obs.center_x, obs.center_y, self.min_x, self.min_y, row
                )

                if self.obstacle_map.get(obs_id) is None:
                    self.obstacle_map[obs_id] = []
                # self.__calculate_rectangle(obs)
                self.obstacle_map[obs_id].append(obs)
        return row

    def __no_split_area(self):
        """暴力融合，没有分区
        """
        self.obstacle_map = {}
        self.obstacle_map[0] = []
        for vm in self.vehicle_messages:
            for obs in vm.obstacles:
                # TODO 计算rect前进行坐标转换
                obs.center_x, obs.center_y, obs.angle = transform(
                    obs.longitude, obs.latitude, 0  # obs.angle
                )
                # self.__calculate_rectangle(obs)
                self.obstacle_map[0].append(obs)

    def __smaller_merge(self):
        """对分割后区域的障碍物包围框进行融合
        """
        from utils.getIntersectionArea import get_IOU
        # 遍历map的keys
        for om in self.obstacle_map:
            # 按置信度从大到小排序
            self.obstacle_map[om].sort(reverse=True)
            # om_len = len(self.obstacle_map[om])
            start = 0
            for ib in self.obstacle_map[om]:
                # it = ib+1
                # while it < om_len:
                # 安全，如果ib+1>om_len，则跳出循环
                start += 1
                if not ib.generate_rect:
                    ib.calculate_rectangle()
                for it in self.obstacle_map[om][start:]:
                    # 比较IOU，如果交并比大于阈值0.6，那么认为这两个bbox是一个，移除置信度较低的
                    if not it.generate_rect:
                        it.calculate_rectangle()
                    iou = get_IOU(ib.rectangle, it.rectangle)
                    if iou > 0.1:
                        # self.obstacle_list.append(ib)
                        self.obstacle_map[om].remove(it)

            for it in self.obstacle_map[om]:
                self.obstacle_list.append(it)

    def __bigger_merge(self, row=0):
        """对分割后的区域，按相邻4个为单位进行融合，每次步进一个分区，解决相邻分区内无法融合的情况
        TODO 分区冗余去重
        """
        for om in self.obstacle_map:
            temp_list = []
            temp_list.extend([obs for obs in self.obstacle_map[om]])
            if self.obstacle_map.get(om + 1) != None:
                temp_list.extend([obs for obs in self.obstacle_map[om + 1]])
            if self.obstacle_map.get(om + row) != None:
                temp_list.extend([obs for obs in self.obstacle_map[om + row]])
            if self.obstacle_map.get(om + row + 1) != None:
                temp_list.extend(
                    [obs for obs in self.obstacle_map[om + row + 1]])
            self.__merge(temp_list)

    def __merge(self, obstacle_list: List[Obstacle]):
        from utils.getIntersectionArea import get_IOU
        obstacle_list.sort(reverse=True)
        # om_len = len(self.obstacle_map[om])
        start = 0
        for ib in obstacle_list:
            # it = ib+1
            # while it < om_len:
            # 安全，如果ib+1>om_len，则跳出循环
            start += 1
            if not ib.generate_rect:
                ib.calculate_rectangle()
            for it in obstacle_list[start:]:
                # 比较IOU，如果交并比大于阈值0.6，那么认为这两个bbox是一个，移除置信度较低的
                if not it.generate_rect:
                    it.calculate_rectangle()
                iou = get_IOU(ib.rectangle, it.rectangle)
                if iou > 0.6:
                    # self.obstacle_list.append(ib)
                    obstacle_list.remove(it)

        for it in obstacle_list:
            self.obstacle_list.append(it)

    def __to_dict(self):
        # obstacles 去重
        # self.obstacle_list = list(set(self.obstacle_list))
        for ol in self.obstacle_list:
            self.obstacle_dict.append(ol.get_dict())


def parseObstacle(obj: Dict):
    return Obstacle(
        length=obj["length"],
        width=obj["width"],
        heading=obj["obj_heading"],
        classification=obj["classification"],
        score=obj["score"],
        longitude=obj["obj_longitude"],
        latitude=obj["obj_latitude"],
        height=obj["height"],
        speed_x_object=obj["speed_x_object"],
        speed_y_object=obj["speed_y_object"],
    )


# TODO 合并两个Json解析
def parse_vehicle_json(root):
    """parse Json of vehicle
    解析车辆传来的Json字符串

    Args:
        root (str): vehicle obstacles

    Returns:
        list (Obstacle): list of Obstacle
    """
    res = []
    for obj in root["object"]:
        obstacle = parseObstacle(obj)
        res.append(obstacle)
    return (res, root["car_info"][0])


def parse_rsu_json(root):
    """parse Json of RSU
    解析路侧传来的Json字符串

    Args:
        root (str): RSU obstacles

    Returns:
        list (Obstacle): list of Obstacle
    """
    res = []
    if root.get("object") != None:
        for obj in root["object"]:
            obstacle = parseObstacle(obj)
            # pretty_print(obstacle)
            res.append(obstacle)
    elif root.get("spot") != None:
        res = root["spot"]
    return res


def CloudMerge(**args):
    """解析args中的Json串，转换成障碍物列表，进行融合，将融合结果和停车位信息整合到一个字典中
    rsu_data: Json
    vehicle_data: Json
    parking_data: Json, NOTE 停车位信息在传参时必须指明！

    Usage:
    res = CloudMerge(parking_data=parking_data,
                     rsu_data=rsu_data, vehicle_data=vehicle_data)

    Returns:
        dict : {
            "obstacle": list of Obstacle,
            "parking": list of parking space
            }
    """
    msgs = []
    parking_data = None
    res_dict = {}
    for i in args:
        root = args[i]
        if root.get("rsu_id") != None:
            objs = parse_rsu_json(root)
            if i == "parking_data":
                parking_data = parse_rsu_json(root)
                continue
        elif root.get("vin") != None:
            objs, car_info = parse_vehicle_json(root)

        msg = VehicleMessage(obstacles=objs)
        msgs.append(msg)
    merge_obstacle = MergeObstacles(vehicle_messages=msgs)
    merge_obstacle.obstacles_merge()
    res_dict["obstacles"] = merge_obstacle.obstacle_dict
    res_dict["parking"] = parking_data
    # res_dict['car_info'] = CarInfo(car_info).get_dict()
    car_info = CarInfo(car_info)
    res_dict["brakehold"] = car_info.get_brakehold()
    res_dict["gear"] = car_info.get_gear()
    res_dict["car"] = car_info.get_car()
    return res_dict


if __name__ == "__main__":
    # usage example

    # rsu_data = {"object":[{"angle":203.14696921163417,"classification":1,"height":1.3565864562988281,"length":4.2797970771789551,"obj_latitude":43.833547963506845,"obj_longitude":125.15435299405293,"object_id":0,"score":0.83527565002441406,"speed_object":0.0,"width":1.6920744180679321},{"angle":203.14696921163417,"classification":1,"height":1.4834309816360474,"length":3.6536417007446289,"obj_latitude":43.833531152958194,"obj_longitude":125.15434218813793,"object_id":0,"score":0.75636047124862671,"speed_object":0.0,"width":1.576163649559021},{"angle":203.14696921163417,"classification":1,"height":1.3680644035339355,"length":4.1221656799316406,"obj_latitude":43.833494783694256,"obj_longitude":125.15431908903307,"object_id":0,"score":0.74195212125778198,"speed_object":0.0,"width":1.5455571413040161},{"angle":203.14696921163417,"classification":1,"height":1.4589201211929321,"length":4.561302661895752,"obj_latitude":43.833568751301961,"obj_longitude":125.15436798724687,"object_id":0,"score":0.73650681972503662,"speed_object":0.0,"width":1.7629247903823853},{"angle":203.14696921163417,"classification":1,"height":1.3868716955184937,"length":4.4567737579345703,"obj_latitude":43.833560168156389,"obj_longitude":125.15436096140485,"object_id":0,"score":0.73336070775985718,"speed_object":0.0,"width":1.6876707077026367},{"angle":203.14696921163417,"classification":1,"height":1.3905938863754272,"length":3.2121069431304932,"obj_latitude":43.833475803306136,"obj_longitude":125.15430711442174,"object_id":0,"score":0.65271842479705811,"speed_object":0.0,"width":1.5075463056564331},{"angle":203.14696921163417,"classification":1,"height":1.4263720512390137,"length":3.7772047519683838,"obj_latitude":43.833512043782626,"obj_longitude":125.15433018126508,"object_id":0,"score":0.64610892534255981,"speed_object":0.0,"width":1.5816782712936401},{"angle":203.14696921163417,"classification":1,"height":1.6687897443771362,"length":3.9453926086425781,"obj_latitude":43.833531249866752,"obj_longitude":125.15434446326285,"object_id":0,"score":0.46660199761390686,"speed_object":0.0,"width":1.6394491195678711}],"rsu_id":0,"rsu_latitude":43.833643356000003,"rsu_longtitude":125.15441118,"timestamp":1618909049888.0,"type":7}
    # vehicle_data = {"car_info":[{"brake":1,"car_speed":0.0,"gear":0,"heading":86.59999847412109,"lateraaccel":0.08999999999999986,"latitude":43.83354390,"longtitude":125.15595910,"longtitudinalaccl":-0.01999999999999957,"parking_status":3,"wheel_angle":-11.30000019073486}],"lane_array":"null","obstacle":[{"angle":1.503534095454353,"classification":1,"height":1.349340319633484,"length":4.013907909393311,"obj_heading":91.3234863281250,"obj_longitude":125.1558576714584,"obj_latitude":43.83359153151689,"object’_id":0,"score":0.9509484171867371,"speed_object":0.0,"width":1.569068908691406},{"angle":0.4739125617248485,"classification":1,"height":1.420738220214844,"length":4.210854053497314,"obj_heading":88.08883666992188,"obj_longitude":125.1560520006726,"obj_latitude":43.83356471860214,"object’_id":0,"score":0.9297859668731689,"speed_object":0.0,"width":1.621722817420959},{"angle":1.458361045667668,"classification":1,"height":1.390147686004639,"length":3.742442607879639,"obj_heading":91.18157196044922,"obj_longitude":125.1559215756832,"obj_latitude":43.83358503506166,"object’_id":0,"score":0.9059512019157410,"speed_object":0.0,"width":1.556316256523132},{"angle":0.5092627370111850,"classification":1,"height":1.491156339645386,"length":4.689557075500488,"obj_heading":88.19989776611328,"obj_longitude":125.1556256317268,"obj_latitude":43.83360523951410,"object’_id":0,"score":0.8687685132026672,"speed_object":0.0,"width":1.647013068199158},{"angle":1.433113630855670,"classification":1,"height":1.444103479385376,"length":3.644472599029541,"obj_heading":91.10225677490234,"obj_longitude":125.1559518746025,"obj_latitude":43.83357976138956,"object’_id":0,"score":0.8440003991127014,"speed_object":0.0,"width":1.588649749755859},{"angle":0.5012451953449564,"classification":1,"height":1.354986786842346,"length":4.230749130249023,"obj_heading":88.17470550537109,"obj_longitude":125.1557287956392,"obj_latitude":43.83360044647844,"object’_id":0,"score":0.8434959053993225,"speed_object":0.0,"width":1.556746244430542},{"angle":0.4944543556536620,"classification":1,"height":1.365940451622009,"length":4.244527816772461,"obj_heading":88.15337371826172,"obj_longitude":125.1558902598240,"obj_latitude":43.83358550809302,"object’_id":0,"score":0.8306603431701660,"speed_object":0.0,"width":1.586012125015259},{"angle":0.4968209403100806,"classification":1,"height":1.617732167243958,"length":4.535519599914551,"obj_heading":88.16080474853516,"obj_longitude":125.1558214001188,"obj_latitude":43.83359296280783,"object’_id":0,"score":0.7979535460472107,"speed_object":0.0,"width":1.685321807861328},{"angle":1.494788114113143,"classification":1,"height":1.495181083679199,"length":3.723340511322021,"obj_heading":91.29601287841797,"obj_longitude":125.1555911671491,"obj_latitude":43.83360212405959,"object’_id":0,"score":0.7164645195007324,"speed_object":0.0,"width":1.583813309669495},{"angle":0.4534665801718923,"classification":1,"height":1.366805434226990,"length":3.676541090011597,"obj_heading":88.02460479736328,"obj_longitude":125.1561189008583,"obj_latitude":43.83356239928486,"object’_id":0,"score":0.6660506129264832,"speed_object":0.0,"width":1.584865450859070},{"angle":0.5226241427930409,"classification":1,"height":1.363372683525085,"length":4.057646751403809,"obj_heading":88.24186706542969,"obj_longitude":125.1560194856176,"obj_latitude":43.83357705434398,"object’_id":0,"score":0.6546686291694641,"speed_object":0.0,"width":1.582932710647583},{"angle":0.8527779564525876,"classification":1,"height":1.419901609420776,"length":4.154132843017578,"obj_heading":89.27908325195312,"obj_longitude":125.1563527172979,"obj_latitude":43.8333178347470,"object’_id":0,"score":0.6440813541412354,"speed_object":0.0,"width":1.595504283905029}],"timestamp":1618562507764.0,"type":1,"vin":"001"}
    # res = CloudMerge(vehicle_data=vehicle_data, rsu_data=rsu_data)
    # print(res)

    rsu_data = {
        "object": [
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.3565864562988281,
                "length": 4.2797970771789551,
                "obj_latitude": 43.833547963506845,
                "obj_longitude": 125.15435299405293,
                "object_id": 0,
                "score": 0.83527565002441406,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.6920744180679321,
            },
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.4834309816360474,
                "length": 3.6536417007446289,
                "obj_latitude": 43.833531152958194,
                "obj_longitude": 125.15434218813793,
                "object_id": 0,
                "score": 0.75636047124862671,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.576163649559021,
            },
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.3680644035339355,
                "length": 4.1221656799316406,
                "obj_latitude": 43.833494783694256,
                "obj_longitude": 125.15431908903307,
                "object_id": 0,
                "score": 0.74195212125778198,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.5455571413040161,
            },
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.4589201211929321,
                "length": 4.561302661895752,
                "obj_latitude": 43.833568751301961,
                "obj_longitude": 125.15436798724687,
                "object_id": 0,
                "score": 0.73650681972503662,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.7629247903823853,
            },
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.3868716955184937,
                "length": 4.4567737579345703,
                "obj_latitude": 43.833560168156389,
                "obj_longitude": 125.15436096140485,
                "object_id": 0,
                "score": 0.73336070775985718,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.6876707077026367,
            },
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.3905938863754272,
                "length": 3.2121069431304932,
                "obj_latitude": 43.833475803306136,
                "obj_longitude": 125.15430711442174,
                "object_id": 0,
                "score": 0.65271842479705811,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.5075463056564331,
            },
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.4263720512390137,
                "length": 3.7772047519683838,
                "obj_latitude": 43.833512043782626,
                "obj_longitude": 125.15433018126508,
                "object_id": 0,
                "score": 0.64610892534255981,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.5816782712936401,
            },
            {
                "obj_heading": 203.14696921163417,
                "classification": 1,
                "height": 1.6687897443771362,
                "length": 3.9453926086425781,
                "obj_latitude": 43.833531249866752,
                "obj_longitude": 125.15434446326285,
                "object_id": 0,
                "score": 0.46660199761390686,
                "speed_x_object": 0.0,
                "speed_y_object": 0.0,
                "width": 1.6394491195678711,
            },
        ],
        "rsu_id": 0,
        "rsu_latitude": 43.833643356000003,
        "rsu_longtitude": 125.15441118,
        "timestamp": 1618909049888.0,
        "type": 7,
    }

    vehicle_data = {
        "timestamp": 1504535566231,
        "type": 1,
        "vin": 1,
        "lane": [
            {
                "lane_id": "001",
                "point_x": [1, 2, 3, 4, 5, 6, 7, 8, 9],
                "point_y": [1, 2, 3, 4, 5, 6, 7, 8, 9],
            },
            {
                "lane_id": "002",
                "point_x": [1, 2, 3, 4, 5, 6, 7, 8, 9],
                "point_y": [1, 2, 3, 4, 5, 6, 7, 8, 9],
            },
        ],
        "object": [
            {
                "object_id": "001",
                "classification": 1,
                "width": 4.032,
                "length": 1.6101,
                "height": 1.368,
                "angle": -105.37513081162906,
                "obj_heading": 21.888094426984253,
                "score": 20,
                "speed_x_object": 10,
                "speed_y_object": 10,
                "obj_longitude": 125.15581517226055,
                "obj_latitude": 43.833470008219976,
            },
            {
                "object_id": "002",
                "classification": 1,
                "width": 3.5537,
                "length": 1.4935,
                "height": 1.4631,
                "angle": -8.509046431855628,
                "obj_heading": 23.969833633166616,
                "score": 20,
                "speed_x_object": 10,
                "speed_y_object": 10,
                "obj_longitude": 125.15618915357521,
                "obj_latitude": 43.83356508813449,
            },
            {
                "object_id": "003",
                "classification": 1,
                "width": 3.9394,
                "length": 1.5463,
                "height": 1.3978,
                "angle": -20.073152279847726,
                "obj_heading": 198.6246896920219,
                "score": 20,
                "speed_x_object": 10,
                "speed_y_object": 10,
                "obj_longitude": 125.15600803265986,
                "obj_latitude": 43.8335155636145,
            },
            {
                "object_id": "004",
                "classification": 1,
                "width": 3.9998,
                "length": 1.5377,
                "height": 1.3718,
                "angle": -23.43199910203153,
                "obj_heading": 200.86182253201116,
                "score": 20,
                "speed_x_object": 10,
                "speed_y_object": 10,
                "obj_longitude": 125.15597670887136,
                "obj_latitude": 43.83350956990844,
            },
            {
                "object_id": "005",
                "classification": 1,
                "width": 4.0522,
                "length": 1.5356,
                "height": 1.4266,
                "angle": -15.246757177553235,
                "obj_heading": 203.16594586737995,
                "score": 20,
                "speed_x_object": 10,
                "speed_y_object": 10,
                "obj_longitude": 125.1560685288965,
                "obj_latitude": 43.833529224472116,
            },
            {
                "object_id": "006",
                "classification": 1,
                "width": 4.301,
                "length": 1.6009,
                "height": 1.4983,
                "angle": -13.729065944786841,
                "obj_heading": 23.79127199769499,
                "score": 20,
                "speed_x_object": 10,
                "speed_y_object": 10,
                "obj_longitude": 125.15610421028782,
                "obj_latitude": 43.83353585459786,
            },
            {
                "object_id": "007",
                "classification": 1,
                "width": 4.7029,
                "length": 1.6114,
                "height": 1.3582,
                "angle": -41.75756902907709,
                "obj_heading": 23.536198583140845,
                "score": 20,
                "speed_x_object": 10,
                "speed_y_object": 10,
                "obj_longitude": 125.15591255450127,
                "obj_latitude": 43.83348927722945,
            },
        ],
        "car_info": [
            {
                "car_speed": 10,
                "longtitudinalaccl": 12.45,
                "lateraaccel": 12.45,
                "longitude": 132,
                "latitude": 44,
                "brake": 1,
                "gear": 1,
                "parking_status": 1,
                "heading": 23,
                "wheel_angle": 200,
            }
        ],
    }
    parking_data = {
        "rsu_id": 0,
        "longtitude": 0,
        "latitude": 0,
        "timestamp": 0,
        "type": 8,
        "spot": [{"spot_id": 1, "occupied": 0}, {"spot_id": 1, "occupied": 0}],
    }
    res = CloudMerge(
        rsu_data=rsu_data, vehicle_data=vehicle_data, parking_data=parking_data
    )
    print(res)

    # test example
    # import sys
    # import pandas as pd
    # csv_file = sys.path[0] + "/../merged.csv"
    # data = pd.read_csv(csv_file, usecols=["counter_time", "classificaton", "score", "center_x",
    #                                       "center_y", "height", "length", "width", "obj_longitude",
    #                                       "obj_latitude", "heading"])
    # data = np.array(data)
    # list_test = []
    # for i in range(data.shape[0]):
    #     obj = Obstacle(length=data[i][6], width=data[i][7], heading=data[i][10],
    #                    longtitude=0, latitude=0, classification=data[i][1], score=data[i][2],
    #                    speed=0, height=0)
    #     obj.set_position(center=(data[i][3], data[i][4]))
    #     # pretty_print(obj)
    #     list_test.append(obj)
    # msg = VehicleMessage(obstacles=list_test)
    # msgs = []
    # msgs.append(msg)
    # mo_test = MergeObstacles(msgs)
    # mo_test.obstacles_merge()
    # res_test = mo_test.obstacle_list
    # print(len(res_test), '/', len(list_test))
    # print(len(res_test['obstacles']))
    # save_csv(res_test, 'test_merged.csv')
    pass
